close all;
clear;

%% Configuration
%expName ='track_2';
expName ='E2';

dataPath = strcat('../Data/',expName,'/');
numCam = 6;

addpath(dataPath);                  addpath('../Data/caliPara');   addpath('./graph_clustering');    
addpath('./CoreFunction');      addpath('./IDManagement');
seqData = dir(strcat(dataPath,'*.txt'));    seqNum  = size(seqData,1);

%% Initialization
worldPoints = [];

% Visualization
h  = figure;
iBase = 0.3;
for iR=0:3
    for iG=0:3
        for iB=0:3
            color{iR*9+iG*3+iB+1} = [iR*iBase iG*iBase iB*iBase ];
        end
    end
end
symList = {'^','o','<','>','p','x','+','.','o','*','d','h'};
plotPos = [2 2;2 0;1 0;0 0;0 2;1 2;1 1];

% Orientation
oriR = 1;
deltaThreshold = 0.2;

% Association
slideWindowSZ = 10;
maxPeoplePKinect = 4;
clusterIdxCur = zeros(numCam*maxPeoplePKinect,1);
clusterIdxPre = ones(numCam*maxPeoplePKinect,1)*(-1);
disThres = 0.2;

member2ID = ones(numCam*maxPeoplePKinect,1)*(-1);
numMember = numCam*maxPeoplePKinect;
firstIDPTRValid = 0;
lastIDPTRValid = 0;
lastIDPTRAll = 0;

dataReliability = zeros(numCam*maxPeoplePKinect,1);

% Distance

curDis = zeros(numCam*maxPeoplePKinect,numCam*maxPeoplePKinect,slideWindowSZ);
accDis = zeros(numCam*maxPeoplePKinect,numCam*maxPeoplePKinect);
aveDis = zeros(numCam*maxPeoplePKinect,numCam*maxPeoplePKinect);
denomin = zeros(numCam*maxPeoplePKinect,numCam*maxPeoplePKinect);



% Load data
for iNumCam = 1:numCam
    seqCam{iNumCam} = dir(sprintf('../Data/%s/K%d/*.jpg',expName,iNumCam));
    addpath (sprintf('../Data/%s/K%d',expName,iNumCam));
end

% Data transformation
for iSeqNum = 1:seqNum
    loaded = load(seqData(iSeqNum).name,'-ASCII');
    frameNum = size(loaded,1);
    
    locMat = zeros(frameNum,3);
    cam(iSeqNum).data.point = [loaded(:,4:6) loaded(:,9:11)];
    cam(iSeqNum).data.ID = [loaded(:,3) loaded(:,8)];
    cam(iSeqNum).data.alpha = [loaded(:,7) loaded(:,12)];
    
    if mod(iSeqNum,2)
        cam(iSeqNum).P = load(sprintf('camera%d.Pmat.cal',int8((iSeqNum+1)/2)));
    else
        cam(iSeqNum).P = cam(iSeqNum-1).P;
    end
    [cam(iSeqNum).tK, cam(iSeqNum).tR, cam(iSeqNum).tt, cam(iSeqNum).C] = P2KRtC(cam(iSeqNum).P);
    
    cam(iSeqNum).R = (-cam(iSeqNum).tR)';
    cam(iSeqNum).t = -(-cam(iSeqNum).tR)'*(-cam(iSeqNum).tt);
    
    mask = (cam(iSeqNum).data.point~=0);
    
    cam(iSeqNum).dataW1.point = (cam(iSeqNum).R*cam(iSeqNum).data.point(:,1:3)'+cam(iSeqNum).t*ones(1,frameNum))';
    cam(iSeqNum).dataW2.point = (cam(iSeqNum).R*cam(iSeqNum).data.point(:,4:6)'+cam(iSeqNum).t*ones(1,frameNum))';
    cam(iSeqNum).dataW.point = [cam(iSeqNum).dataW1.point cam(iSeqNum).dataW2.point] .* mask; % Remove the zeros reading
    
    cam(iSeqNum).dataW1.ori = (cam(iSeqNum).R*...
        ( cam(iSeqNum).data.point(:,1:3)'+...
        [-oriR*sin(cam(iSeqNum).data.alpha(:,1)*pi/180) zeros(frameNum,1) oriR*cos(cam(iSeqNum).data.alpha(:,1)*pi/180)]')+...
        cam(iSeqNum).t*ones(1,frameNum))';
    cam(iSeqNum).dataW2.ori = (cam(iSeqNum).R*...
        ( cam(iSeqNum).data.point(:,4:6)'+...
        [-oriR*sin(cam(iSeqNum).data.alpha(:,2)*pi/180) zeros(frameNum,1) oriR*cos(cam(iSeqNum).data.alpha(:,2)*pi/180)]')+...
        cam(iSeqNum).t*ones(1,frameNum))';
    cam(iSeqNum).dataW.ori = [cam(iSeqNum).dataW1.ori cam(iSeqNum).dataW2.ori] .* mask;
    worldPoints(1:frameNum,(iSeqNum-1)*6+1:(iSeqNum-1)*6+6) = cam(iSeqNum).dataW.point;
    
    cam(iSeqNum).dataW.oriEstimatedStatus = false(frameNum,2);
    cam(iSeqNum).dataW.oriEstimatedSupport = true(1,2); % true, support; false, oppse
    
end

%% Main

% ID changes within one camera
preID = zeros(seqNum,2);
curID = zeros(seqNum,2);

iSlideWindow = 0;
for iFrameNum = 1:frameNum
    
%     if iFrameNum ==1548
%         disp('yes');
%     end
    iSlideWindow = mod(iSlideWindow,slideWindowSZ);
    iSlideWindow = iSlideWindow+1;
    
    clf;
    
    curFramePoint = reshape(worldPoints(iFrameNum,:,:),3,numCam*maxPeoplePKinect)';
    
    % Swap the ID if new ID added/removed
    for iSeqNum=1:seqNum
        curID(iSeqNum,:) = cam(iSeqNum).data.ID(iFrameNum,:);
        for iDataSet = 1:2
            if (curID(iSeqNum,iDataSet)~=0) &&...
                    (curID(iSeqNum,iDataSet)~=preID(iSeqNum,iDataSet)) && ...
                    (curID(iSeqNum,iDataSet)==preID(iSeqNum,3-iDataSet))
                
                t = cam(iSeqNum).dataW.point(iFrameNum,1:3);
                cam(iSeqNum).dataW.point(iFrameNum,1:3) = cam(iSeqNum).dataW.point(iFrameNum,4:6);
                cam(iSeqNum).dataW.point(iFrameNum,4:6) = t;
                curFramePoint((iSeqNum-1)*2+iDataSet,:) = curFramePoint((iSeqNum-1)*2+3-iDataSet,:);
                curFramePoint((iSeqNum-1)*2+3-iDataSet,:) = t;
                
                t = cam(iSeqNum).dataW.ori(iFrameNum,1:3);
                cam(iSeqNum).dataW.ori(iFrameNum,1:3) = cam(iSeqNum).dataW.ori(iFrameNum,4:6);
                cam(iSeqNum).dataW.ori(iFrameNum,4:6) = t;
                
                tID = curID(iSeqNum,1);
                curID(iSeqNum,1) = curID(iSeqNum,2);
                curID(iSeqNum,2) = tID;
                
            end
        end
        preID(iSeqNum,:) = curID(iSeqNum,:);
    end
    
    if iFrameNum>3
        % Calculate the orientation by tracking history
        for iSeqNum=1:seqNum
            for iDataSet = 1:2
                deltaX = cam(iSeqNum).dataW.point(iFrameNum,1+(iDataSet-1)*3) - cam(iSeqNum).dataW.point(iFrameNum-3,1+(iDataSet-1)*3);
                deltaY = cam(iSeqNum).dataW.point(iFrameNum,3+(iDataSet-1)*3) - cam(iSeqNum).dataW.point(iFrameNum-3,3+(iDataSet-1)*3);
                cam(iSeqNum).dataW.oriEstimated(iDataSet) = atan2(deltaY,-deltaX);
                if ((abs(deltaX)>deltaThreshold) || (abs(deltaY)>deltaThreshold)) &&...
                        (curID(iSeqNum,iDataSet)~=0) && (curID(iSeqNum,iDataSet)==preID(iSeqNum,iDataSet))
                    cam(iSeqNum).dataW.oriEstimatedStatus(iFrameNum,iDataSet) = true;
                end
            end
        end
    end
    
    % Sensor Reliability Score
    for iSeqNum=1:seqNum
        for iDataSet = 1:2
            dataReliability((iSeqNum-1)*2+iDataSet) = Reliability([cam(iSeqNum).data.point(iFrameNum,1+(iDataSet-1)*3), cam(iSeqNum).data.point(iFrameNum,3+(iDataSet-1)*3)]);
        end
    end
    
    curFramePoint(:,2) = [];

    % calculate the distance matrix
    for iSeqNum=1:seqNum
        curDis((iSeqNum-1)*2+1,(iSeqNum-1)*2+2,iSlideWindow) = inf;    % same camera, INF
        curDis((iSeqNum-1)*2+2,(iSeqNum-1)*2+1,iSlideWindow) = inf;    % same camera, INF
        
        
        for iDataSet = 1:2
            iDisIdx = (iSeqNum-1)*2+iDataSet;
            curDis(iDisIdx,iDisIdx,iSlideWindow) = inf;    % self, INF
            
            for jSeqNum = (iSeqNum+1):seqNum
                for jDataSet = 1:2
                    jDisIdx = (jSeqNum-1)*2+jDataSet;
                    curDis(iDisIdx,jDisIdx,iSlideWindow) = sum((cam(iSeqNum).dataW.point(iFrameNum,[1+(iDataSet-1)*3,3+(iDataSet-1)*3])-cam(jSeqNum).dataW.point(iFrameNum,[1+(jDataSet-1)*3,3+(jDataSet-1)*3])).^2);
                    
                    if ~(any(cam(iSeqNum).data.point(iFrameNum,[1+(iDataSet-1)*3,3+(iDataSet-1)*3])) && any(cam(jSeqNum).data.point(iFrameNum,[1+(jDataSet-1)*3,3+(jDataSet-1)*3])))
                        curDis(iDisIdx,jDisIdx,iSlideWindow) = inf;
                        curDis(jDisIdx,iDisIdx,iSlideWindow) = inf;                        
                    else
                        curDis(iDisIdx,jDisIdx,iSlideWindow) = sum((cam(iSeqNum).dataW.point(iFrameNum,[1+(iDataSet-1)*3,3+(iDataSet-1)*3])-cam(jSeqNum).dataW.point(iFrameNum,[1+(jDataSet-1)*3,3+(jDataSet-1)*3])).^2);
                        curDis(jDisIdx,iDisIdx,iSlideWindow) = curDis(iDisIdx,jDisIdx,iSlideWindow);
                    end
                    
                end
            end
        end
    end
    
    % normalize the distance
    nextIdx = mod(iSlideWindow,slideWindowSZ);
    nextIdx = nextIdx+1;
    
    accDis = accDis + curDis(:,:,iSlideWindow) - curDis(:,:,nextIdx);
    denomin = (denomin<5)+denomin;
    tMask = isinf(accDis)|isnan(accDis);
    
    aveDis = accDis./denomin;
    
    IDNew = 0;
    IDNewIdx = 1;
    
    % Cluster assignment
    for iSeqNum=1:seqNum
        for iDataSet = 1:2
            
            iDisIdx = (iSeqNum-1)*2+iDataSet;
            
            tIdxV = aveDis(iDisIdx,:)<disThres; %Average
            %tIdxV = curDis(iDisIdx,:,iSlideWindow)<disThres; % Current
            
            
            tIdx = find(tIdxV==1, 1, 'first');
            [tmin tMinIdx(iDisIdx)] = min(aveDis(iDisIdx,:));
            %[tmin tIdx] = min(curDis(iDisIdx,:,1));
            if isempty(tIdx) || (~any(cam(iSeqNum).data.point(iFrameNum,[1+(iDataSet-1)*3,3+(iDataSet-1)*3])))
                clusterIdxCur(iDisIdx) = -1;
                IDNew(iDisIdx) = -1;
                
            else
                clusterIdxCur(iDisIdx) = min(iDisIdx,tIdx);
                IDNew(iDisIdx) = IDNewIdx;
                IDNewIdx = IDNewIdx+1;
            end
        end
    end
    % Cluster ID Check
    for iMem=1:seqNum*2
        % if is not in the same cluster of the nearest node
        if (clusterIdxCur(iMem)~=-1) && (clusterIdxCur(iMem)~=clusterIdxCur(tMinIdx(iMem)))
            clusterIdxCur(iMem) = min(iMem,clusterIdxCur(tMinIdx(iMem)));
            clusterIdxCur(tMinIdx(iMem)) = clusterIdxCur(iMem);
        end
    end
    
    % ID assignment
    for iMem=1:seqNum*2
        % 1. clustering changed        
        % ID changed
        if (clusterIdxCur(iMem)~= clusterIdxPre(iMem) ) && (member2ID(iMem)~=-1)
            % 1.1 lost track, remove the member
            %                 clusterID = clusterIdxPre(iMem);
            %                 deleteID = member2ID(clusterID);
            deleteID = member2ID(iMem);
            if ID(deleteID).memberCnt>1
                % update ID
                ID(deleteID).memberList(iMem) = false;
                ID(deleteID).memberCnt = ID(deleteID).memberCnt-1;
            else
                % remove ID
                ID(deleteID).status = false;
                
                if deleteID ==lastIDPTRValid
                    lastIDPTRValid = 0;
                    for iClusterID = deleteID-1:-1:firstIDPTRValid
                        if ID(iClusterID).status
                            lastIDPTRValid = iClusterID;
                            break;
                        end
                    end
                end
                
                if deleteID==firstIDPTRValid
                    firstIDPTRValid = 0;
                    for iClusterID = deleteID+1:lastIDPTRValid
                        if ID(iClusterID).status
                            firstIDPTRValid = iClusterID;
                            break;
                        end
                    end
                end
            end
            %member2ID(iMem) = -1;
        end
        
        if clusterIdxCur(iMem)~=-1
            % 1.2 new member
            clusterID = clusterIdxCur(iMem);
            newID = member2ID(clusterID);
            if clusterID == iMem
                newID = -1;
            end
            
            % update with the previous IDs
            for jMem = 1:seqNum*2
                if (iMem~=jMem)&&(clusterIdxCur(iMem)== clusterIdxCur(jMem)) && (member2ID(jMem)~=-1) && (member2ID(jMem))
                    newID = member2ID(jMem);
                    break;
                end
            end
            
            if newID~=-1 && (~ID(newID).memberList(iMem))
                % 1.2.1 add to existing ID
                ID(newID).memberList(iMem) = true;
                ID(newID).memberCnt = ID(newID).memberCnt + 1;
                member2ID(iMem) = newID;
                
            elseif newID==-1
                % 1.2.2 search for the previous IDs
                preFlag = false;
                for iID = 1:lastIDPTRAll
                    if ~ID(iID).status
                        if IDMatching(iMem,iID,ID,clusterIdxCur)
                            preFlag = true;
                            newID = iID;
                            break;
                        end
                    end
                end
                
                if preFlag
                    % reuse previous ID
                    %ID(1).feature = [];
                    firstIDPTRValid = min(firstIDPTRValid,newID);
                    lastIDPTRValid = max(lastIDPTRValid,newID);
                else
                    % create new ID
                    newID = lastIDPTRAll+1;
                    if firstIDPTRValid ==0
                        firstIDPTRValid = 1;
                    end
                    lastIDPTRValid = newID;
                    lastIDPTRAll = newID;
                end
                
                ID(newID).memberList = false(numMember,1);
                ID(newID).memberList(iMem) = true;
                ID(newID).memberCnt = 1;
                ID(newID).status = true;
                member2ID(iMem) = newID;
            end
        end
    end
    
    
    for iMem=1:seqNum*2
        if clusterIdxCur(iMem)==-1
            member2ID(iMem) = -1;
        end
    end
    
    accDis(tMask)=0;
    denomin(tMask)=0;
    
    for iS = 1:slideWindowSZ
        tCurD = curDis(:,:,iS);
        tCurD(tMask)=0;
        curDis(:,:,iS) = tCurD;
    end
    
    % Merge with the reliability
    for iID=1:lastIDPTRAll
        if ID(iID).status
            minR = -1;
            for iMem = 1:numMember
                if ID(iID).memberList(iMem) && dataReliability(iMem)>minR
                    minR = dataReliability(iMem);
                end
            end
        end
        ID(iID).reliability = minR;
    end
    
    
    %% Visualization
    % Show original images
    for iNumCam = 1:numCam
        img = imread(seqCam{iNumCam}(iFrameNum).name);
        img = flipdim(img,2);
        h(iNumCam) = subplot(3,3,iNumCam);
        imshow(img);
        hold on;
        text(50,50,sprintf('Cam%d',iNumCam),'Color',color{iNumCam*4},'FontWeight','Bold','FontSize',30);
        
    end
    h(7) = subplot(3,3,7);
    
    for iSeqNum=1:seqNum
        plot3(cam(iSeqNum).C(1),cam(iSeqNum).C(2),cam(iSeqNum).C(3),[color{iSeqNum},'o'],'linewidth',10);
        hold on;
        
        % plot viewing axes
        axis_dir = cam(iSeqNum).R(:,3);
        axis_len = 1;
        endpoint = cam(iSeqNum).C+axis_len*axis_dir;
        
        line([cam(iSeqNum).C(1),endpoint(1)],...
            [cam(iSeqNum).C(2),endpoint(2)],...
            [cam(iSeqNum).C(3),endpoint(3)],'linewidth',7,'Color',color{iSeqNum});%,[C(2,i),endpoint(2)],[C(3,i),endpoint(3)]);
        hold on;
        
        if mod(iSeqNum,2)
            text(cam(iSeqNum).C(1),cam(iSeqNum).C(2),cam(iSeqNum).C(3)+0.3,sprintf('%4d',int8((iSeqNum+1)/2)),'Color',color{iSeqNum},'FontSize',20);
            hold on;
        end
        
        for iDataSet = 1:2
            colorIdx = member2ID((iSeqNum-1)*2+iDataSet);
            
            %colorIdx = clusterIdxCur((iSeqNum-1)*2+iDataSet);
            %IIID((iSeqNum-1)*2+iDataSet)+1
            if(clusterIdxCur((iSeqNum-1)*2+iDataSet)~=-1 && colorIdx~= -1)
                colorSelected = color{colorIdx};
                if dataReliability((iSeqNum-1)*2+iDataSet)< ID(member2ID((iSeqNum-1)*2+iDataSet)).reliability
                    continue;
                    colorSelected = [0.8 0.8 0.8];
                end
                
                plot3(cam(iSeqNum).dataW.point(iFrameNum,1+(iDataSet-1)*3),...
                    cam(iSeqNum).dataW.point(iFrameNum,2+(iDataSet-1)*3),...
                    cam(iSeqNum).dataW.point(iFrameNum,3+(iDataSet-1)*3),'Color',colorSelected,'Marker',symList{iDataSet},'linewidth',15);
                
                % direction correction
                dirXP = cam(iSeqNum).dataW.ori(iFrameNum,1+(iDataSet-1)*3);
                dirYP = cam(iSeqNum).dataW.ori(iFrameNum,3+(iDataSet-1)*3);
                
                if iFrameNum>3 && ...
                        cam(iSeqNum).dataW.oriEstimatedStatus(iFrameNum-1,iDataSet) && ...
                        cam(iSeqNum).dataW.oriEstimatedStatus(iFrameNum-2,iDataSet) && ...
                        cam(iSeqNum).dataW.oriEstimatedStatus(iFrameNum-3,iDataSet)
                    
                    oriKinect = atan2(dirYP, dirXP);
                    if abs(oriKinect - cam(iSeqNum).dataW.oriEstimated(iDataSet)) > (pi/2)
                        cam(iSeqNum).dataW.oriEstimatedSupport(iDataSet) = false;
                    else
                        cam(iSeqNum).dataW.oriEstimatedSupport(iDataSet) = true;
                    end
                    
                end
                
                if ~cam(iSeqNum).dataW.oriEstimatedSupport(iDataSet)
                    % for test
                    line([cam(iSeqNum).dataW.point(iFrameNum,1+(iDataSet-1)*3) dirXP],...
                        [0 0],...
                        [cam(iSeqNum).dataW.point(iFrameNum,3+(iDataSet-1)*3) dirYP]...
                        ,'linewidth',5,'Color','k');
                    
                    dirXP = 2*cam(iSeqNum).dataW.point(iFrameNum,1+(iDataSet-1)*3) - dirXP;
                    dirYP = 2*cam(iSeqNum).dataW.point(iFrameNum,3+(iDataSet-1)*3) - dirYP;
                end
                
                % direction
                line([cam(iSeqNum).dataW.point(iFrameNum,1+(iDataSet-1)*3) dirXP],...
                    [0 0],...
                    [cam(iSeqNum).dataW.point(iFrameNum,3+(iDataSet-1)*3) dirYP]...
                    ,'linewidth',5,'Color','r');
                
                %text(cam(iSeqNum).C(1),cam(iSeqNum).C(2),cam(iSeqNum).C(3)+0.3,sprintf('%4d',int8((iSeqNum+1)/2)),'Color',color{iSeqNum},'FontSize',20);
                text(cam(iSeqNum).dataW.point(iFrameNum,1+(iDataSet-1)*3)+1,...
                    cam(iSeqNum).dataW.point(iFrameNum,2+(iDataSet-1)*3)+0.5,...
                    cam(iSeqNum).dataW.point(iFrameNum,3+(iDataSet-1)*3),sprintf('Cam%d-%d',int32(iSeqNum/2),iDataSet),'Color','r','FontSize',20);
                hold on;
            end
        end
    end
    axis([-4,4,0,6,-0.15,4.2]);        title(sprintf('Frame %d',iFrameNum));
    set(gca,'CameraTarget',[0,0,0]);    set(gca,'CameraPosition',[0,1,0]);
    xlabel('X');    zlabel('Z');    grid on;
    
    %Set image layout
    for iNumCam = 1:numCam
        set(h(iNumCam),'position',[0.33*plotPos(iNumCam,1) 0.33*plotPos(iNumCam,2) 0.33 0.33]);
    end
    set(h(7),'position',[0.32 0.15 0.35 0.35]);
    
    pause(0.1);
    
    % Clustering Idx
    clusterIdxPre = clusterIdxCur;
    clusterIdxCur = zeros(numCam*maxPeoplePKinect,1);
end
